from dronekit import connect, VehicleMode
import time
from pymavlink import mavutil
import math

# 改为当前连接的飞控的端口
connection_string = '10.42.0.10:14550'
print('Connecting to vehicle on: %s' % connection_string)
# connect函数将会返回一个Vehicle类型的对象，即此处的vehicle
# 即可认为是无人机的主体，通过vehicle对象，我们可以直接控制无人机
vehicle = connect(connection_string, wait_ready=True, baud=921600)


# 高度以米为单位
def arm_and_takeoff(aTargetAltitude):
    """
    Arms vehicle and fly to aTargetAltitude.
    """
    #  进行起飞前检查
    print("Basic pre-arm checks")

    # 解锁无人机（电机将开始旋转）
    print("Arming motors")
    # 将无人机的飞行模式切换成"GUIDED"（一般建议在GUIDED模式下控制无人机）
    vehicle.mode = VehicleMode("GUIDED")
    # 通过设置vehicle.armed状态变量为True，解锁无人机
    vehicle.armed = True

    # 在无人机起飞之前，确认电机已经解锁
    while not vehicle.armed:
        print(" Waiting for arming...")
        time.sleep(1)

    # 发送起飞指令
    print("Taking off!")
    # simple_takeoff将发送指令，使无人机起飞并上升到目标高度
    # send_takeoff_command(aTargetAltitude)
    vehicle.simple_takeoff(aTargetAltitude)
    # # 在无人机上升到目标高度之前，阻塞程序
    while True:
        print(" Altitude: ", vehicle.location.global_relative_frame.alt)
        # 当高度上升到目标高度的0.95倍时，即认为达到了目标高度，退出循环
        # vehicle.location.global_relative_frame.alt为相对于home点的高度
        if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95:
            print("Reached target altitude")
            break
            # 等待1s
        time.sleep(1)


# 飞行方向速度 * 时间 = 距离
# x表示前后方向,y表示左右方向,z表示垂直高度方向
def send_nav_velocity(velocity_x, velocity_y, velocity_z, duration):
    # 生成SET_POSITION_TARGET_LOCAL_NED命令
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,  # time_boot_ms (not used)
        0, 0,  # target system, target component
        mavutil.mavlink.MAV_FRAME_BODY_NED,  # frame
        0b0000111111000111,  # type_mask (only speeds enabled)
        0, 0, 0,  # x, y, z positions (not used)
        velocity_x, velocity_y, velocity_z,  # x, y, z velocity in m/s
        0, 0, 0,  # x, y, z acceleration (not used)
        0, 0)  # yaw, yaw_rate (not used)
    for x in range(0, duration):
        vehicle.send_mavlink(msg)
        time.sleep(1)


def fly_circle(radius):
    """
    让无人机飞一个完整的圆形轨迹。

    :param radius: 圆的半径（单位：米）。
    """
    # 确保半径不超过1.5米
    if radius > 1.5:
        radius = 1.5
        print("Radius limited to 1.5 meters.")
    # 设置飞行速度为0.3米/秒
    speed = 0.3
    # 圆周速度和角速度的关系
    angular_velocity = speed / radius
    # 计算圆周长
    circumference = 2 * math.pi * radius
    # 计算飞行一个完整圆形所需的时间
    duration = circumference / speed
    # 速度分量
    velocity_z = 0  # 假设保持当前高度不变
    # 计算时间步长
    time_step = 1  # 每隔1秒更新一次速度指令
    # 开始飞行圆形轨迹
    start_time = time.time()
    while time.time() - start_time < duration:
        # 计算当前角度
        current_angle = angular_velocity * (time.time() - start_time)
        # 计算速度分量
        velocity_x = speed * math.cos(current_angle)
        velocity_y = speed * math.sin(current_angle)
        # 发送速度指令
        send_nav_velocity(velocity_x, velocity_y, velocity_z, time_step)
        # 等待下一个时间步长
        time.sleep(time_step)
    print("圆形执行完成")


arm_and_takeoff(1)
fly_circle(2)
